#include "RobotBase.h"
#include "RobotHMHModify.h"
#include "RobotAntiEntropy.h"
#include "RobotPheromone.h"
#include "stage.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

using namespace Stg;

int PositionUpdate( Model* mod, RobotBase* robot );
void ProcessMessage( WifiMessageBase * mesg);

RobotBase * RobotRef[200];

// Stage calls this when the model starts up
extern "C" int Init( Model* mod, CtrlArgs* args )
{
    RobotBase* robot;
    
    // Read algorithm type
    char *alg = getenv("ALGORITHM");
    if(!alg)
    {
        printf("Fuck! Can't find Algorithm. Go to hell!!!\n");
        return 1;
    }

    printf("Get algorithm type is %s\n", alg);
    if(strcmp(alg, "HMHModify") == 0)
        robot = new RobotHMHModify(mod);
    else if(strcmp(alg, "AntiEntropy") == 0)
        robot = new RobotAntiEntropy(mod);
    else if(strcmp(alg, "Pheromone") == 0)
        robot = new RobotPheromone(mod);
    else
    {
        printf("Fuck! Can't find Algorithm. Go to hell!!!\n");
        return 1;
    }

    // Read movement method
    char *move = getenv("MOVEMENT");
    if(!move)
    {
        printf("Fuck! Can't find movement method. Go to hell!!!\n");
        return 1;
    }

    printf("Get movement method is %s\n", move);
    if(strcmp(move, "Rand") == 0)
        robot->movementType = 1;
    else if(strcmp(move, "Straight") == 0)
        robot->movementType = 2;
    else if(strcmp(move, "Static") == 0)
        robot->movementType = 0;
    else
    {
        printf("Fuck! Can't find movement method. Go to hell!!!\n");
        return 1;
    }

	RobotRef[robot->id] = robot;

	robot->position->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot );


	robot->laser->Subscribe(); // starts the laser updates
	robot->position->Subscribe(); // starts the position updates
	robot->wifi->Subscribe();
	robot->wifi->comm.SetReceiveMsgFn(ProcessMessage);

	return 0;
}

void ProcessMessage( WifiMessageBase * mesg )
{	
	int RecipientId = mesg->GetRecipientId();
	
	RobotRef[RecipientId/4 - 1]->MessageReceive(mesg);

}

// call the robot's loop function
int PositionUpdate( Model* mod, RobotBase* robot )
{
	robot->DoLoop();
	return 0; // run again
}
